#include "TilingScheme.h"
#include "CesiumMath.h"
#include "Cartesian2.h"
#include "Cartesian3.h"
#include "Cartesian4.h"
#include "Matrix3.h"
#include "Matrix4.h"
#include "Spherical.h"
#include "Cartographic.h"
#include "Quaternion.h"
#include "Ellipsoid.h"
#include "Transforms.h"
#include "Rectangle.h"
#include "EllipsoidGeodesic.h"

namespace OCPP
{
	namespace Cesium
	{
		const Rectangle Rectangle::MAX_VALUE = Rectangle(-CesiumMath::PI,
			-CesiumMath::PI_OVER_TWO,
			CesiumMath::PI,
			CesiumMath::PI_OVER_TWO);

		HeadingPitchRoll HeadingPitchRoll::fromQuaternion(Quaternion quaternion) {
			HeadingPitchRoll result;
			auto test = 2 * (quaternion.w * quaternion.y - quaternion.z * quaternion.x);
			auto denominatorRoll =
				1 - 2 * (quaternion.x * quaternion.x + quaternion.y * quaternion.y);
			auto numeratorRoll =
				2 * (quaternion.w * quaternion.x + quaternion.y * quaternion.z);
			auto denominatorHeading =
				1 - 2 * (quaternion.y * quaternion.y + quaternion.z * quaternion.z);
			auto numeratorHeading =
				2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y);
			result.heading = -atan2(numeratorHeading, denominatorHeading);
			result.roll = atan2(numeratorRoll, denominatorRoll);
			result.pitch = -CesiumMath::asinClamped(test);
			return result;
		};

		const Cartographic Cartographic::ZERO = Cartographic(0.0, 0.0, 0.0);
		const Cartesian3 Cartographic::wgs84OneOverRadii = Cartesian3(
			1.0 / 6378137.0,
			1.0 / 6378137.0,
			1.0 / 6356752.3142451793
		);
		const Cartesian3 Cartographic::wgs84OneOverRadiiSquared = Cartesian3(
			1.0 / (6378137.0 * 6378137.0),
			1.0 / (6378137.0 * 6378137.0),
			1.0 / (6356752.3142451793 * 6356752.3142451793)
		);

		const Cartesian2 Cartesian2::UNIT_X = Cartesian2(1.0, 0.0);
		const Cartesian2 Cartesian2::UNIT_Y = Cartesian2(0.0, 1.0);

		const Cartesian3 Cartesian3::ZERO = Cartesian3(0.0, 0.0, 0.0);
		const Cartesian3 Cartesian3::ONE = Cartesian3(1.0, 1.0, 1.0);
		const Cartesian3 Cartesian3::UNIT_X = Cartesian3(1.0, 0.0, 0.0);
		const Cartesian3 Cartesian3::UNIT_Y = Cartesian3(0.0, 1.0, 0.0);
		const Cartesian3 Cartesian3::UNIT_Z = Cartesian3(0.0, 0.0, 1.0);

		const Cartesian3 Cartesian3::wgs84RadiiSquared(
			6378137.0 * 6378137.0,
			6378137.0 * 6378137.0,
			6356752.3142451793 * 6356752.3142451793
		);

		Cartesian3 Cartesian3::multiplyByScalar(Quaternion cartesian, double scalar) {
			Cartesian3 result;
			result.x = cartesian.x * scalar;
			result.y = cartesian.y * scalar;
			result.z = cartesian.z * scalar;
			return result;
		};

		Cartesian2 Cartesian2::fromCartesian3(Cartesian3 cartesian3)
		{
			return fromElements(cartesian3.x, cartesian3.y);
		}

		Cartesian2 Cartesian2::fromCartesian4(Cartesian4 cartesian4)
		{
			return fromElements(cartesian4.x, cartesian4.y);
		}

		Cartesian3 Cartesian3::fromCartesian4(Cartesian4 cartesian4)
		{
			return fromElements(cartesian4.x, cartesian4.y, cartesian4.z);
		}

		const Matrix3 Matrix3::IDENTITY = Matrix3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
		const Matrix3 Matrix3::ZERO = Matrix3(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);

		/**
			 * Computes a 3x3 rotation matrix from the provided quaternion.
			 *
			 * @param {Quaternion} quaternion the quaternion to use.
			 * @param {Matrix3} [result] The object in which the result will be stored, if undefined a new instance will be created.
			 * @returns {Matrix3} The 3x3 rotation matrix from this quaternion.
			 */
		Matrix3 Matrix3::fromQuaternion(Quaternion quaternion) {
			auto x2 = quaternion.x * quaternion.x;
			auto xy = quaternion.x * quaternion.y;
			auto xz = quaternion.x * quaternion.z;
			auto xw = quaternion.x * quaternion.w;
			auto y2 = quaternion.y * quaternion.y;
			auto yz = quaternion.y * quaternion.z;
			auto yw = quaternion.y * quaternion.w;
			auto z2 = quaternion.z * quaternion.z;
			auto zw = quaternion.z * quaternion.w;
			auto w2 = quaternion.w * quaternion.w;

			auto m00 = x2 - y2 - z2 + w2;
			auto m01 = 2.0 * (xy - zw);
			auto m02 = 2.0 * (xz + yw);

			auto m10 = 2.0 * (xy + zw);
			auto m11 = -x2 + y2 - z2 + w2;
			auto m12 = 2.0 * (yz - xw);

			auto m20 = 2.0 * (xz - yw);
			auto m21 = 2.0 * (yz + xw);
			auto m22 = -x2 - y2 + z2 + w2;

			return Matrix3(m00, m01, m02, m10, m11, m12, m20, m21, m22);
		};

		const static Matrix4 IDENTITY(1.0,
			0.0,
			0.0,
			0.0,
			0.0,
			1.0,
			0.0,
			0.0,
			0.0,
			0.0,
			1.0,
			0.0,
			0.0,
			0.0,
			0.0,
			1.0);
		const static Matrix4 ZERO(0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0,
			0.0);

		const IntVector Matrix3::rowVal = { 1, 0, 0 };
		const IntVector Matrix3::colVal = { 2, 2, 1 };

		////
		const Cartesian4 Cartesian4::ZERO = Cartesian4(0.0, 0.0, 0.0, 0.0);
		const Cartesian4 Cartesian4::ONE = Cartesian4(1.0, 1.0, 1.0, 1.0);
		const Cartesian4 Cartesian4::UNIT_X = Cartesian4(1.0, 0.0, 0.0, 0.0);
		const Cartesian4 Cartesian4::UNIT_Y = Cartesian4(0.0, 1.0, 0.0, 0.0);
		const Cartesian4 Cartesian4::UNIT_Z = Cartesian4(0.0, 0.0, 1.0, 0.0);
		const Cartesian4 Cartesian4::UNIT_W = Cartesian4(0.0, 0.0, 0.0, 1.0);

		///
		const Quaternion Quaternion::ZERO = Quaternion(0.0, 0.0, 0.0, 0.0);
		const Quaternion Quaternion::IDENTITY = Quaternion(0.0, 0.0, 0.0, 1.0);
	}
}